/* 
 * File:   ApproximateBeliefUpdate.h
 * Author: Hadrien
 *
 * Created on 11 juillet 2012, 14:24
 */

#ifndef APPROXIMATEBELIEFUPDATE_H
#define	APPROXIMATEBELIEFUPDATE_H

template<typename EXACTBELIEFUPDATE>
class ApproximateBeliefUpdate {
    
public:
    typedef EXACTBELIEFUPDATE u_belief_t;
    typedef typename u_belief_t::belief_t belief_t;
    typedef typename belief_t::super_state_t super_state_t;
    typedef typename super_state_t::state_t state_t;
    typedef typename super_state_t::obs_t obs_t;
    typedef typename super_state_t::action_t action_t;
    ApproximateBeliefUpdate(){
        
    }
    virtual ~ApproximateBeliefUpdate(){
        
    }
    
    belief_t operator()(const belief_t& b, const action_t& a, const obs_t& z, const int& K) const{
        //I did the first line to show you how use the template
//        belief_t bb = tau(b,a,z);
        
        kvector kVector;
        
        std::cout << std::endl << std::endl;

        belief_t newBelief;
        vector Sb = b.getStates();
        double sum = 0;
        int a_index = a.index();
        int z_index = z.index();
        for(vector_it it = Sb.begin(); it != Sb.end(); ++it) { // Iterate over all non-zero states
            
            int s = it->getState().index();
            for(int ss = 0; ss < state_t::size; ss++) { // Iterate over all possible next states
                
                super_state_t newSState = it->update(a_index,ss,z_index); // Retrieves superstate
                
                // Probabiliy increment for superstate being considered
                double inc = b.get(*it)*it->getT(s,a_index,ss)*it->getO(ss,a_index,z_index);
                
                // If increment equals zero -> no action, else 
                if(inc>0){
                    
                    // First loop : is the superstate already in the list, retrieves prob, erase
                    
                    double newB = inc;
                    bool inserted = false;
                    
                    for (kvector_it it2 = kVector.begin(); it2 != kVector.end(); it2++) {

                        super_state_t supstate = it2->first;
                        if (supstate == newSState)
                        {
                            
                            newB += it2->second;
                            kVector.erase(it2);
                            break;
                        }
                            
                    }
                    
                    if (kVector.size() < K && kVector.size() > 0){
                    
                        // Second loop : inserts at right place
                        for (kvector_it it3 = kVector.begin(); it3 != kVector.end(); it3++) {
     
                            if (newB > it3->second)
                            {
                                kVector.insert(it3,std::make_pair<super_state_t,double>(newSState,newB));
                                inserted = true;
                                break;
                            }
                            
                        }
                        
                    }
                    
                    int n = kVector.size();
                    
                    // Inserts at last position if required
                    if (n < K && !inserted){
                        kVector.push_back(std::make_pair<super_state_t,double>(newSState,newB));
                    }
                    
//                    
//                    for (kvector_it it2 = kVector.begin(); it2 != kVector.end(); it2++) {
//                        
//                        std::cout << it2->first << " / " << it2->second << std::endl;
//                    }
//                    
//                    std::cout << "=======================================" << std::endl;
          
                    
                }
            }
        }
        
        for (kvector_it it4 = kVector.begin(); it4 != kVector.end(); it4++)
        {
            
            newBelief.set(it4->first, it4->second);
            sum += it4->second;
            
            
        }
        
        //Normalized        
        newBelief.normalize(sum);
        
        for (kvector_it it2 = kVector.begin(); it2 != kVector.end(); it2++) {
            
            std::cout << it2->first << " / " << it2->second/sum << std::endl;
        }
        
        std::cout << "=======================================" << std::endl;
        
        
        
        
        return newBelief;
        
    }
private:
    u_belief_t tau;
    typedef typename std::vector<super_state_t> vector;
    typedef typename vector::const_iterator vector_it;
    typedef typename std::vector<std::pair<super_state_t,double> > kvector;
    typedef typename kvector::iterator kvector_it;
    
    
};

#endif	/* APPROXIMATEBELIEFUPDATE_H */

